% Given the arm trajectory of a 2joint arm in end-point coordinates,
% this code can calculate the joint torques; INVERSE dynamics.
% See appendix B in Trainin et al 2007.
%
% Using 2DOF arm sim derivation from Trainin et al, 2007, without the
% muscles.  I'm dealing with joint torques directly for simplicity and to
% match a robot arm rather than an animal arm.
% Trying to ignore gravity for now, as they did.
%
%   th2 |
% ------- elbow
%       |
%       |th1  = shoulder angle
% ---------------


F= 135; % Force in newtons. kg m/s^2
theta1 = 0.0; % straight out from body.
theta2 = 0.0; % straight out from body.
L1 = .144; % length of arm segment in meters.
L2 = .154;
M1 = .29; % mass of arm segment in kg.
M2 = .25;
I1 = 2.2e-4; % inertia of arm segment in kg m^2
I2 = 6.7e-4;

theta_max = 130*pi/180;

tau = 1/100;
sim_seconds = 10;

theta1_vel = 0;
theta2_vel = 0;

theta1s=zeros( 1, ceil(sim_seconds/tau) );
theta2s=zeros( 1, ceil(sim_seconds/tau) );

ts = tau:tau:sim_seconds;
for t = 1:ceil(sim_seconds/tau);

%  The following was for a single joint with gravity (see motor1d.m): 
%    tq_muscle = F*r*sin(theta+theta_offset);
% 
%    tq_load = g*sin(theta)*forearm_length*.5*forearm_mass
% 
%    tq_net = tq_muscle - tq_load;
% 
%    theta_accel = tq_net/(forearm_mass*(forearm_length*.5)^2);


   % calculate torques

   Fex = [0 1]';
   % Jacobian
   J = [-L1*sin(theta1)-L2*sin(theta1+theta2), -L2*sin(theta1+theta2);L1*cos(theta1)+L2*cos(theta1+theta2),  L2*cos(theta1+theta2)];
   
   % static torque
   
   Tq_static = J'*Fex;
   
   % dynamic torque (no gravity for now.  Arm in horizontal plane.)
   
   Tq_dynamic(1) = h11*
   
   
   % total torque
   Tq = -Tq_static - Tq_dynamic;
   
   
   theta1_vel = theta_vel + tau* theta_accel;
   theta1 = theta1 + tau* theta1_vel;

   
   % apply limits
   if theta1 < 0
       theta1 = 0;
   end
   if theta1 > theta_max
       theta1 = theta_max;
   end
   if theta2 < 0
       theta2 = 0;
   end
   if theta2 > theta_max
       theta2 = theta_max;
   end

   % save
   theta1s(t) = theta1;
   theta2s(t) = theta1;

   % elbow
   xe = L1*cos(theta1);
   ye = L1*sin(theta1);

   % hand
   xh = xe + L2*cos(theta1+theta2);
   yh = ye + L2*sin(theta1+theta2);

   % draw
   plot([0 xe xh],[0 ye yh]);
   axis([0 L1+L2 0 L1+L2]);
   drawnow;
    
   
end


plot(ts,theta1*180/pi);


xe = L1*cos(theta1s) + L2*cos(theta1s+theta2s);
ye = L1*sin(theta1s) + L2*sin(theta1s+theta2s);




